#!/usr/bin/env python
import roslib; roslib.load_manifest('enclosure_escape')
import rospy
from geometry_msgs.msg import Twist
from irobot_create_2_1.msg import SensorPacket
# global variables 
bump = False

# listen (adapted from line_follower
def processSensing(sensorPacket):
    global bump
    bump = sensorPacket.bumpLeft or sensorPacket.bumpRight
    #newInfo = True

def hello_create():
    pub = rospy.Publisher('cmd_vel', Twist)
    rospy.Subscriber('sensorPacket', SensorPacket, processSensing)
    rospy.init_node('hello_create')
    #listen
    global bump
    twist = Twist()
    while not rospy.is_shutdown():
        if bump:
            str = "hello create, you have bumped into something  %s"%rospy.get_time()
            rospy.loginfo(str)
            twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 2
 `          bump = False
        else:
            str = "hello create, you can spin now %s"%rospy.get_time()
            rospy.loginfo(str)
            twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0.1
        pub.publish(twist)
        rospy.sleep(1.0)
if __name__ == '__main__':
    try:
        hello_create()
    except rospy.ROSInterruptException: pass

